/*
 * @file charge.cpp
 * @author YinWeibin
 * @date 1 15, 2024
 * @brief charger operation
 * Contact: Weibin.yin@lgmgim.cn
 */
// charge_controller.cpp
#include "battery/charge.hpp"

ChargeController::ChargeController() {
}

ChargeController::~ChargeController() {
    running_ = false;
    if (thread_.joinable()) thread_.join();
    if (liftDriver_thread_.joinable()) liftDriver_thread_.join();
    if (can_socket_ >= 0) close(can_socket_);
}

void ChargeController::init(std::shared_ptr<rclcpp::Node> node) {
    node_ = node;
    // Parameters
    node_->declare_parameter("retractInplaceSensor", false);
    node_->declare_parameter("target_ms", 5000);
    node_->declare_parameter("retractInPlace_ms", 500);
    node_->declare_parameter("bit_index", -1);
    node_->declare_parameter("battery_type", "");
    node_->declare_parameter("chargePlateNeedFall", false);

    retractSensorEnable_ = node_->get_parameter("retractInplaceSensor").as_bool();
    target_ms_ = node_->get_parameter("target_ms").as_int();
    retractInPlace_pt_ms_ = node_->get_parameter("retractInPlace_ms").as_int();
    bit_index_ = node_->get_parameter("bit_index").as_int();
    battery_type_ = node_->get_parameter("battery_type").as_string();
    chargePlateNeedFall_ = node_->get_parameter("chargePlateNeedFall").as_bool();

    RCLCPP_INFO(node_->get_logger(), "Parameters loaded: retract=%d, target_ms=%d", 
                retractSensorEnable_, target_ms_);

    // Initialize CAN
    if (battery_type_ == "rebot") {
        std::string can_device = "can1";
        node_->declare_parameter("base/battery_can_device", "can1");
        can_device = node_->get_parameter("base/battery_can_device").as_string();

        struct sockaddr_can addr;
        struct ifreq ifr;
        if ((can_socket_ = socket(AF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
            RCLCPP_FATAL(node_->get_logger(), "Socket creation failed");
            return;
        }

        std::strncpy(ifr.ifr_name, can_device.c_str(), IFNAMSIZ - 1);
        ifr.ifr_name[IFNAMSIZ - 1] = '\0';
        
        if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) {
            RCLCPP_FATAL(node_->get_logger(), "I/O control failed");
            close(can_socket_);
            can_socket_ = -1;
            return;
        }

        addr.can_family = AF_CAN;
        addr.can_ifindex = ifr.ifr_ifindex;

        if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
            RCLCPP_FATAL(node_->get_logger(), "Bind failed");
            close(can_socket_);
            can_socket_ = -1;
            return;
        }
        RCLCPP_INFO(node_->get_logger(), "CAN initialized on %s", can_device.c_str());
    }

    // ROS2 setup
    io_pub_ = node_->create_publisher<agv_msgs::msg::OutPutsNew>("/setOutput", 10);
    agvmode_sub_ = node_->create_subscription<agv_msgs::msg::AGVMode>(
        "/agvmode", 1, std::bind(&ChargeController::agvmodeCallback, this, std::placeholders::_1));
    eventOccurSub_ = node_->create_subscription<agv_msgs::msg::AGVEventStatus>(
        "/eventstatus", 1, std::bind(&ChargeController::eventCallback, this, std::placeholders::_1));
    inputNewSub_ = node_->create_subscription<agv_msgs::msg::ReadInPutsNew>(
        "/readInputNew", 1, std::bind(&ChargeController::inputCallback, this, std::placeholders::_1));
    auto_run_sub_ = node_->create_subscription<agv_msgs::msg::AgvAutoStatus>(
        "/auto_run_status", 1, std::bind(&ChargeController::auto_statusCallback, this, std::placeholders::_1));
    trigger_server_ = node_->create_service<agv_srvs::srv::BatteryCharge>(
        "/do_charge", std::bind(&ChargeController::chargeCB, this, std::placeholders::_1, std::placeholders::_2));
    outputclientNew_ = node_->create_client<agv_srvs::srv::OutputsNew>("/outputNew");
    
    if (chargePlateNeedFall_) {
        periphTeleop_sub_ = node_->create_subscription<agv_msgs::msg::PeriphTeleop>(
            "/base/periphTeleopInfo", 1, std::bind(&ChargeController::periphTeleopCallback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "Charge plate lift enabled");
    }

    // Start threads
    thread_ = std::thread(&ChargeController::thread_fun, this);
    if (chargePlateNeedFall_) {
        liftDriver_thread_ = std::thread(&ChargeController::liftDriver_thread_fun, this);
    }
}

// ===================== Callbacks =====================
void ChargeController::agvmodeCallback(const agv_msgs::msg::AGVMode::SharedPtr state_p) {
    mode_ = *state_p;
}

void ChargeController::auto_statusCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg) {
    auto_run_ = *msg;
}

void ChargeController::periphTeleopCallback(const agv_msgs::msg::PeriphTeleop::SharedPtr msg) {
    teleopCode_.Update(msg->order);
}

void ChargeController::eventCallback(const agv_msgs::msg::AGVEventStatus::SharedPtr event) {
    event_ = *event;
}

void ChargeController::inputCallback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg) {
    inputState_get(*msg, "chargeAccept", inputs_.retractInplaceSensor);
    inputState_get(*msg, "resetButton", inputs_.resetBtn);
    inputState_get(*msg, "plateHigh", inputs_.high_pos);
    inputState_get(*msg, "plateLow", inputs_.low_pos);
}

void ChargeController::chargeCB(
    const agv_srvs::srv::BatteryCharge::Request::SharedPtr req,
    agv_srvs::srv::BatteryCharge::Response::SharedPtr resp) 
{
    start_ = req->start;
    resp->success = true;
    resp->message = "Charge command processed";
    RCLCPP_DEBUG(node_->get_logger(), "Received charge cmd: %s", req->start ? "start" : "end");
}

// ===================== Helper Functions =====================
void ChargeController::inputState_get(const agv_msgs::msg::ReadInPutsNew &msg, 
                                     const std::string& key, bool &state) 
{
    for (const auto &data : msg.array) {
        if (data.key == key) {
            state = (data.value != 0);
            break;
        }
    }
}

bool ChargeController::isManual() {
    return (mode_.mode == agv_msgs::msg::AGVMode::MANUAL);
}

bool ChargeController::OnChargePoint() {
    return (auto_run_.fromnode != DEFAUT_POINT_ID && 
            auto_run_.tonode == auto_run_.fromnode &&
            auto_run_.workstate == agv_msgs::msg::AgvAutoStatus::IS_IDLE &&
            !auto_run_.driving &&
            mode_.mode == agv_msgs::msg::AGVMode::AUTOMATIC &&
            bit_index_ >= 0 && bit_index_ <= 15 &&
            TestBit(auto_run_.pointbits, bit_index_));
}

void ChargeController::set_charge(bool state) {
    OnChargePoint_ = state;
}

void ChargeController::callOutputSrv(const std::string& outStr, bool needOpen) {
    if (!outputclientNew_->service_is_ready()) {
        RCLCPP_ERROR(node_->get_logger(), "Output service not available");
        return;
    }

    auto request = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
    agv_msgs::msg::PairTypeInt out;
    out.key = outStr;
    out.value = needOpen ? 1 : 0;
    request->array.push_back(out);

    auto result = outputclientNew_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(
        node_->get_node_base_interface(), result, 500ms) == rclcpp::FutureReturnCode::SUCCESS) 
    {
        RCLCPP_DEBUG(node_->get_logger(), "Set %s to %d success", outStr.c_str(), needOpen);
    } else {
        RCLCPP_ERROR(node_->get_logger(), "Set %s to %d failed", outStr.c_str(), needOpen);
    }
}

void ChargeController::setOnePoint(bool level, const std::string& key) {
    agv_msgs::msg::PairTypeInt point;
    point.key = key;
    point.value = level ? 1 : 0;
    outPuts_.push_back(point);
}

void ChargeController::clearOutputs() {
    outPuts_.clear();
}

bool ChargeController::outputsPub() {
    if (outPuts_.empty()) return false;
    
    auto msg = std::make_unique<agv_msgs::msg::OutPutsNew>();
    msg->array = outPuts_;
    io_pub_->publish(std::move(msg));
    return true;
}

bool ChargeController::timer_ON(bool level, int target_ms) {
    using namespace std::chrono;
    static auto start_time = steady_clock::now();
    static bool last_level = false;
    
    if (level && !last_level) { // Rising edge
        start_time = steady_clock::now();
    }
    
    last_level = level;
    
    if (!level) return false;
    
    auto elapsed = duration_cast<milliseconds>(steady_clock::now() - start_time);
    return (elapsed.count() >= target_ms);
}

void ChargeController::platePosCheck() {
    if ((state_ == IDLE || mode_.mode == agv_msgs::msg::AGVMode::MANUAL) && !inputs_.high_pos) {
        eventManger_.updateEvent(
            agv_msgs::msg::AGVEvent::CHARGEPLATE_WRONG_PLACE,
            agv_msgs::msg::AGVEvent::ESTOP,
            agv_msgs::msg::AGVEvent::ALARM,
            "Charge plate not in high position",
            inputs_.high_pos,
            chargePlateNeedFall_
        );
    } else if (eventManger_.checkEvent(agv_msgs::msg::AGVEvent::CHARGEPLATE_WRONG_PLACE)) {
        eventManger_.removeEvent(agv_msgs::msg::AGVEvent::CHARGEPLATE_WRONG_PLACE);
    }
    plateAutoRaiseReq_ = (state_ == IDLE && !inputs_.high_pos && 
                         mode_.mode == agv_msgs::msg::AGVMode::AUTOMATIC);
}

void ChargeController::batteryHeat() {
    if (battery_type_ == "rebot" && can_socket_ >= 0) {
        struct can_frame frame;
        frame.can_id = 0x110;
        frame.can_dlc = 8;
        
        if (state_ == CHARGING_ACTION || state_ == CHARGING_ONPOINT) {
            uint8_t data[8] = {0x01, 0x01, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00};
            memcpy(frame.data, data, 8);
        } else {
            uint8_t data[8] = {0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00};
            memcpy(frame.data, data, 8);
        }
        
        if (write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) {
            RCLCPP_ERROR(node_->get_logger(), "CAN write error");
        }
    }
}

// ===================== Main Thread Functions =====================
void ChargeController::thread_cycle() {
    clearOutputs();
    bool cancel = (mode_.mode != agv_msgs::msg::AGVMode::AUTOMATIC);
    
    if (chargePlateNeedFall_) platePosCheck();
    
    int target_ms = retractSensorEnable_ ? retractInPlace_pt_ms_ : target_ms_;
    bool level = retractSensorEnable_ ? (ending_ && inputs_.retractInplaceSensor) : ending_;
    bool timer_trig = timer_ON(level, target_ms);
    
    switch (state_) {
        case IDLE:
            if (start_ || OnChargePoint_) {
                state_ = chargePlateNeedFall_ ? Fall : SELECT;
                setOnePoint(true, "chargeEnable");
                setOnePoint(true, "chargeIO");
                if (is_batteryheating_) setOnePoint(true, "batteryHeating");
                chargeEnable_ = true;
                chargeRelay_ = true;
                batteryHeating_ = true;
                ending_ = false;
            }
            break;
        case Fall:
            plateFallReq_ = true;
            if (inputs_.low_pos) {
                plateFallReq_ = false;
                state_ = SELECT;
            } else if (cancel) {
                state_ = retractSensorEnable_ ? WAITING_Inplace : WAITING_Timeout;
                chargeEnable_ = false;
            }
            break;
        case SELECT:
            if (start_) {
                state_ = CHARGING_ACTION;
            } else if (OnChargePoint_) {
                state_ = CHARGING_ONPOINT;
            } else {
                state_ = ERROR;
            }
            break;
        case CHARGING_ACTION:
            setOnePoint(true, "chargeEnable");
            setOnePoint(true, "chargeIO");
            if (is_batteryheating_) setOnePoint(true, "batteryHeating");
            if (!start_ || cancel) {
                state_ = WAITING_Inplace;
                start_ = false;
                chargeEnable_ = false;
            }
            break;
        case CHARGING_ONPOINT:
            setOnePoint(true, "chargeEnable");
            setOnePoint(true, "chargeIO");
            if (is_batteryheating_) setOnePoint(true, "batteryHeating");
            if (!OnChargePoint_ || cancel) {
                state_ = WAITING_Inplace;
                chargeEnable_ = false;
            }
            break;
        case WAITING_Inplace:
            ending_ = true;
            setOnePoint(false, "chargeEnable");
            chargeEnable_ = false;
            if (timer_trig) {
                state_ = chargePlateNeedFall_ ? Raise : OVER;
                chargeRelay_ = false;
                batteryHeating_ = false;
            }
            if (cancel) {
                chargeRelay_ = false;
                setOnePoint(false, "chargeIO");
            }
            break;
        case Raise:
            plateRaiseReq_ = true;
            plateFallReq_ = false;
            if (inputs_.high_pos) {
                plateRaiseReq_ = false;
                state_ = OVER;
            }
            break;
        case ERROR:
            chargeEnable_ = false;
            state_ = OVER;
            break;
        case OVER:
            ending_ = false;
            setOnePoint(false, "chargeEnable");
            setOnePoint(false, "chargeIO");
            if (is_batteryheating_) setOnePoint(false, "batteryHeating");
            chargeEnable_ = false;
            chargeRelay_ = false;
            batteryHeating_ = false;
            plateRaiseReq_ = false;
            plateFallReq_ = false;
            state_ = IDLE;
            break;
    }
    
    if (event_.ecode == agv_msgs::msg::AGVEvent::ESTOP) {
        chargeEnable_ = false;
        chargeRelay_ = false;
        batteryHeating_ = false;
        setOnePoint(false, "chargeEnable");
        setOnePoint(false, "chargeIO");
        if (is_batteryheating_) setOnePoint(false, "batteryHeating");
    }
    
    outputsPub();
    batteryHeat();
}

void ChargeController::liftDriver_thread_cycle() {
    static bool raising = false, falling = false;
    bool raise, fall;
    
    if (teleopCode_.TimeOut(TOPIC_TIMEOUT_MS) && teleopCode_.Get() != NO_ACTIVE) {
        teleopCode_.Update(NO_ACTIVE);
    }
    
    lift_timeout_ = ((raising || falling) && cmdValid_.TimeOut(CMD_TIMEOUT_MS)) && !inputs_.resetBtn;
    
    if (lift_timeout_) {
        eventManger_.updateEvent(
            agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT,
            agv_msgs::msg::AGVEvent::STOP,
            agv_msgs::msg::AGVEvent::ALARM,
            "Charge plate lift timeout",
            raising,
            falling
        );
    } else if (eventManger_.checkEvent(agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT)) {
        eventManger_.removeEvent(agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT);
    }
    
    uint8_t opCode = teleopCode_.Get();
    raise = ((opCode == UP && mode_.mode == agv_msgs::msg::AGVMode::MANUAL) ||
            plateRaiseReq_ || plateAutoRaiseReq_) &&
            !inputs_.high_pos && !plateFallReq_ && !lift_timeout_ &&
            event_.ecode != agv_msgs::msg::AGVEvent::ESTOP;
            
    fall = ((opCode == DOWN && mode_.mode == agv_msgs::msg::AGVMode::MANUAL) ||
            plateFallReq_) &&
            !inputs_.low_pos && !raise && !lift_timeout_ &&
            event_.ecode != agv_msgs::msg::AGVEvent::ESTOP;
    
    if (raising != raise) {
        raising = raise;
        callOutputSrv("chargePlateRaise", raise);
        callOutputSrv("chargePlateFall", false);
        cmdValid_.Update();
    }
    
    if (falling != fall) {
        falling = fall;
        cmdValid_.Update();
        callOutputSrv("chargePlateFall", fall);
        callOutputSrv("chargePlateRaise", false);
    }
}

// ===================== Thread Entry Points =====================
void ChargeController::thread_fun() {
    while (rclcpp::ok() && running_) {
        thread_cycle();
        std::this_thread::sleep_for(50ms);
    }
}

void ChargeController::liftDriver_thread_fun() {
    while (rclcpp::ok() && running_) {
        liftDriver_thread_cycle();
        std::this_thread::sleep_for(20ms);
    }
}